clc;
N=100;                          % 迭代次数
x(:,1) = [30;300;20];           % 初始状态：x,v,a
T = 0.5;                        % 采样周期
F = [1, T, 0.5*T^2;             % 转移矩阵，匀加速运动
     0, 1, T;
     0, 0, 1];
Gama = [T^3/6, T^2/2, T]';      % ?噪声系数
H = [1 0 0];
A = x(:,1);
Q=1;
R=100;
for k=2:N 
    w = normrnd(0, 1);
    v = normrnd(0, 10);
    x(:,k) = F*x(:,k-1) + Gama*w;
    A=[A, x(:,k-1)];
    z(:,k) = H*x(:,k) +v;
end

p0 = zeros(3);
x0 = [1;1;1];
x_hat(:,1)=x0;
B=x0;
C=z(:,1);
P=p0;
RR=zeros(1,N);
VV=zeros(1,N);
for m = 2:N 
    xx_hat(:,m-1) = F*x_hat(:,m-1);
    B=[B, xx_hat(:,m-1)];
    zl(:,m) = H*xx_hat(:,m-1);
    C=[C,zl(:,m)];
    PP=F*P*F'+Gama*Q*Gama';
    S=H*PP*H'+R;
    KK=PP*H'*inv(S);
    zl_tutor(:,m) = z(:,m) -C(:,m);
    x_hat(:,m) = xx_hat(:,m-1)+KK*zl_tutor(:,m);
    P=PP-KK*S*KK';
    RR(1,m)=P(1,1);
    VV(1,m)=P(2,2);
end
k=1:N;
figure(1);
plot(k,B(1,k), 'b.', k, A(1, k), 'r.');
legend('估计轨迹','实际轨迹');
figure(2);
plot(k,B(2,k),'b.', k, A(2, k), 'r.');
legend('估计速度', '实际速度');
figure(3);
plot(k, RR, 'b.', k, VV, 'r.');
legend('位置估计方差', '速度估计方差')


